#include "imu_reading/read_imu.h"

namespace imu_serial
{
read_imu::read_imu() : port("/dev/ttyUSB0"),
                       baudrate(115200),
                       timeout(1000),
                       imuDataRead(new IMU_DATA)
{
}

read_imu::~read_imu()
{
  if (imuDataRead != nullptr)
  {
    delete imuDataRead;
  }
  read_serial_thread.detach();
}

bool read_imu::connect(ros::NodeHandle &nh)
{

  //Open Serial
  ImuSerial.setPort(port);
  ImuSerial.setBaudrate(baudrate);
  ImuSerial.setParity(serial::parity_t::parity_none);
  serial::Timeout to = serial::Timeout::simpleTimeout(timeout);
  ImuSerial.setTimeout(to);
  try
  {
    ImuSerial.open();
    ROS_INFO("Serial has been opened!");
  }
  catch (const serial::IOException &e)
  {
    os.str("");
    os << "**Serial** device failed to open, waiting... [" + std::string(e.what()) + "]";
    ROS_WARN_STREAM(os.str());
    is_connect_serial = false;
  }
  catch (const serial::SerialException &e)
  {
    os.str("");
    os << "**Serial** device failed to open, waiting... [" + std::string(e.what()) + "]";
    ROS_WARN_STREAM(os.str());
    is_connect_serial = false;
  }
  catch (const serial::PortNotOpenedException &e)
  {
    os.str("");
    os << "**Serial** device failed to open, waiting... [" + std::string(e.what()) + "]";
    ROS_WARN_STREAM(os.str());
    is_connect_serial = false;
  }

  read_serial_thread = std::thread(boost::bind(&read_imu::read_buffer, this));
  return true;
}

void read_imu::read_buffer()
{
  int rev_len;
  bool find_stx1;
  bool find_stx2;
  uint8_t receive_end;
  uint8_t result0;
  uint8_t result;

  while (ros::ok())
  {
    find_stx1 = false;
    find_stx2 = false;
    ROS_INFO("Finding header byte...");
    while (ros::ok() && (!find_stx1) && (!find_stx2))
    {
      try
      {
        USER_RX_BUF.clear();
        ImuSerial.read(USER_RX_BUF, 1);
        if (USER_RX_BUF[0] == 0x7F)
        {
          find_stx1 = true;
        }
        if (find_stx1)
        {
          ImuSerial.read(USER_RX_BUF, 1);
          if (USER_RX_BUF[1] == 0x80)
          {
            find_stx2 = true;
          }
          else
          {
            find_stx1 = false;
          }
        }
      }
      catch (serial::SerialException e)
      {
        std::cerr << e.what() << std::endl;
      }
    }
    receive_end = 0x01;
    result0 = 0x00;
    result = 0x02;
    try
    {
      ImuSerial.read(USER_RX_BUF, 21);
    }
    catch (serial::SerialException e)
    {
      std::cerr << e.what() << std::endl;
    }
    if (USER_RX_BUF.size() != 23)
    {
      continue;
    }
    ROS_INFO("Receive checking...");
    for (int j = 2; j < 22; j++)
    {
      result0 += USER_RX_BUF[j];
    }
    result = ~result0;
    receive_end = USER_RX_BUF[22];
    if (result != receive_end)
    {
      continue;
    }

    /*  逐一字节读取再计算校验和，会出现提前数据帧提起结束读取
    while (ros::ok() && (result != receive_end))
    {
      try
      {
        ImuSerial.read(USER_RX_BUF, 1);
        rev_len = USER_RX_BUF.size();
        receive_end = USER_RX_BUF[rev_len - 1];
        result0 = USER_RX_BUF[rev_len - 2] + result0;
        result = ~result0;
      }
      catch (serial::SerialException e)
      {
        std::cerr << e.what() << std::endl;
      }
      if (rev_len > 50)
      {
        break;
      }
    }   */

    for (int i = 0; i < USER_RX_BUF.size(); i++)
    {
      printf("%x ", USER_RX_BUF[i]);
    }
    printf("\r\n");

    ParsePacketB4(imuDataRead, USER_RX_BUF);
    // Accel x, y, z
    std::cout << "Accel x, y, z : " << imuDataRead->Ax << ",  " << imuDataRead->Ay << ",  " << imuDataRead->Az << std::endl;
    // Gyro x, y, z
    std::cout << "Gyro x, y, z : " << imuDataRead->Gx << ",  " << imuDataRead->Gy << ",  " << imuDataRead->Gz << std::endl;
    // Roll, Pitch, Yaw
    std::cout << "Roll, Pitch, Yaw : " << imuDataRead->Roll << ",  " << imuDataRead->Pitch << ",  " << imuDataRead->Yaw << ",  " << imuDataRead->Temp << std::endl;
  }
}

// 参数说明
// imuData: 用于存储IMU数据的变量
// buffer: IMU具体数据缓冲区
// length: 这次解析数据的长度
bool read_imu::ParsePacketB4(IMU_DATA *imuData, std::vector<uint8_t> buffer)
{
  if (imuData == NULL || buffer.size() < MinLength)
  {
    return false;
  }
  ROS_INFO("Start decoding...");
  uint16_t sensors[10];
  sensors[1] = (buffer[2] | (buffer[3] << 8));
  sensors[2] = (buffer[4] | (buffer[5] << 8));
  sensors[0] = (buffer[0] | (buffer[1] << 8));
  sensors[3] = (buffer[6] | (buffer[7] << 8));
  sensors[4] = (buffer[8] | (buffer[9] << 8));
  sensors[5] = (buffer[10] | (buffer[11] << 8));
  sensors[6] = (buffer[12] | (buffer[13] << 8));
  sensors[7] = (buffer[14] | (buffer[15] << 8));
  sensors[8] = (buffer[16] | (buffer[17] << 8));
  sensors[9] = (buffer[18] | (buffer[19] << 8));
  ROS_INFO("Get buffer...");
  double factorA = Accel_Scale / (double)Sensor_Scale;
  // Accel x, y, z
  imuData->Ax = static_cast<int>(sensors[0]) * factorA;
  imuData->Ay = (int)sensors[1] * factorA;
  imuData->Az = (int)sensors[2] * factorA;
  // Gyro x, y, z
  double factorR = Rate_Scale / (double)Sensor_Scale;
  imuData->Gx = (int)sensors[3] * factorR;
  imuData->Gy = (int)sensors[4] * factorR;
  imuData->Gz = (int)sensors[5] * factorR;
  // Roll, Pitch, Yaw
  double factorAn = Angle_Scale / (double)Sensor_Scale;
  imuData->Roll = (int)sensors[6] * factorAn;
  imuData->Pitch = (int)sensors[7] * factorAn;
  imuData->Yaw = (int)sensors[8] * factorAn;
  imuData->Temp = (int)sensors[9] * (Temp_Scale / (double)Sensor_Scale);
  return true;
}

} // namespace imu_serial
